import numpy as np

l1 = 0.08
l2 = 0.0315

def forward_kinematics(theta1, theta2,l1, l2):
    """计算机械臂的正向运动学"""
    x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
    y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
    return x, y

def jacobian(theta1, theta2, l1, l2):
    """计算机械臂的雅可比矩阵"""
    J = np.array([
        [-l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2), -l2 * np.sin(theta1 + theta2)],
        [l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2), l2 * np.cos(theta1 + theta2)]
    ])
    return J

def ImpedanceController(target_theta1,target_theta2, current_theta1, current_theta2, current_theta1_velocity,current_theta2_velocity, D=1, K=1):
    """阻抗控制器"""
    # 计算位置误差
    target_position = forward_kinematics(target_theta1, target_theta2, l1, l2)
    print("目标位置:", target_position)
    current_position = forward_kinematics(current_theta1, current_theta2, l1, l2)
    print("当前位置:", current_position)
    position_error = np.array(target_position) - np.array(current_position)
    print("位置误差:", position_error)
    # 计算速度误差
    target_velocity = np.array([0, 0])  # 目标速度为零
    current_velocity = jacobian(current_theta1, current_theta2, l1, l2) @ np.array([current_theta1_velocity, current_theta2_velocity])
    print("当前速度:", current_velocity)
    velocity_error = target_velocity - current_velocity
    print("速度误差:", velocity_error)
    # 计算力
    force = K * position_error + D * velocity_error
    print("计算的力:", force)
    # 计算雅可比矩阵的伪逆
    print("雅可比矩阵", jacobian(current_theta1, current_theta2, l1, l2))
    J_inv = np.linalg.inv(jacobian(current_theta1, current_theta2, l1, l2))
    print("雅可比矩阵的伪逆:", J_inv)
    # 计算关节力矩
    joint_torch = jacobian(current_theta1, current_theta2, l1, l2).T @ force
    print("关节力矩:", joint_torch)

    return joint_torch[0], joint_torch[1]  # 返回关节力矩

if __name__ == "__main__":
    # 测试代码
    target_theta1 = np.pi / 4
    target_theta2 = np.pi / 4
    current_theta1 = np.pi / 6
    current_theta2 = np.pi / 6
    current_theta1_velocity = 0.1
    current_theta2_velocity = 0.1

    joint_velocity = ImpedanceController(target_theta1, target_theta2, current_theta1, current_theta2, current_theta1_velocity, current_theta2_velocity)
    print("关节速度:", joint_velocity)